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ABSTRACT 

The  unscented  transformation  is  applied  to  tracks  given  in  Cartesian  coordi¬ 
nates,  to  obtain  target  range,  bearing,  elevation,  range  rate,  speed  and  course 
estimates,  and  their  associated  errors.  This  approach  is  attractive  because  it 
does  not  require  tedious  closed-form  mathematical  derivations,  is  not  computa¬ 
tionally  expensive,  and  does  not  require  a  pseudo-random  number  generator. 
Example  code  and  results  are  given  to  demonstrate  the  applicability  of  the 
approach. 
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Application  of  the  Unscented  Transformation  to  Obtain 
Target  Range,  Bearing,  Elevation,  Range  Rate,  Speed  and 
Course  Estimates  and  Errors  From  Tracks  Given  in 
Cartesian  Coordinates 


EXECUTIVE  SUMMARY 

Target  tracking  algorithms  typically  produce  tracks  in  Cartesian  coordinates  but  target 
range,  bearing,  elevation  and  other  estimates  may  be  required  to  meet  software  interface 
specifications.  In  this  report,  an  algorithm  based  on  the  unscented  transformation  is  de¬ 
scribed  that  allows  such  estimates  and  their  error  standard  deviations  to  be  computed. 
This  report  is  provided  as  a  contribution  to  the  Defence  Science  and  Technology  Organi¬ 
sation’s  Virtual  Maritime  System  Architecture  (VMSA).  The  VMSA  plays  an  important 
role  in  providing  modelling  and  simulation  support  for  Australian  Defence  projects  such 
as  the  SEA  1448  project  to  upgrade  the  anti-ship  missile  defence  system  of  the  Royal 
Australian  Navy’s  ANZAC  class  frigates. 

Under  the  unscented  transformation  approach,  closed-form  derivations  are  unnecessary 
and  computational  requirements  are  at  least  an  order  of  magnitude  less  than  those  of 
the  Monte  Carlo  method.  Furthermore,  unlike  the  Monte  Carlo  method,  the  unscented 
transformation  does  not  require  a  pseudo-random  number  generator. 

The  basic  idea  of  the  unscented  transformation  is  to  choose  a  set  of  sigma  points  with 
assigned  weights  to  represent  a  multivariate  normal  distribution  that  preserves  the  mean 
and  covariance.  The  points  are  then  transformed,  and  the  transformed  points  are  used  to 
calculate  estimates  of  the  transformed  mean  and  covariance. 

Table  1  on  page  4  of  this  report  summarises  the  algorithm  to  obtain  range,  bearing, 
elevation,  range  rate,  speed  and  course  from  track  data  given  in  Cartesian  coordinates. 

Table  2  on  page  5  of  this  report  compares  example  results  from  the  unscented  trans¬ 
formation  algorithm  with  those  obtained  from  Monte  Carlo  simulations.  The  estimated 
quantities  and  errors  were  found  to  be  in  close  agreement.  Java  code  for  the  example  are 
given  in  the  appendix. 
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1  Introduction 


Target  tracking  algorithms  typically  produce  tracks  in  Cartesian  coordinates  but  target 
range,  bearing,  elevation  and  other  estimates  may  be  required  to  meet  software  interface 
specifications.  In  this  report,  an  algorithm  based  on  the  unscented  transformation  [1]  is 
described  that  allows  such  estimates  and  their  error  standard  deviations  to  be  computed. 
This  report  is  provided  as  a  contribution  to  the  Defence  Science  and  Technology  Organi¬ 
sation’s  Virtual  Maritime  System  Architecture  (VMSA).  The  VMSA  plays  an  important 
role  in  providing  modelling  and  simulation  support  for  Australian  Defence  projects  such 
as  the  SEA  1448  project  to  upgrade  the  anti-ship  missile  defence  system  of  the  Royal 
Australian  Navy’s  ANZAC  class  frigates. 

Track  position  and  velocity  are  given  in  Cartesian  coordinates,  (x,y,z)  and  (. x,y,z ), 
together  with  track  covariance  which  quantifies  the  track  errors.  Obtaining  estimates  of 
target  range,  bearing,  elevation,  range  rate,  speed  and  course,  and  their  error  standard 
deviations,  involves  nonlinear  functions  and  is  consequently  not  straightforward.  The 
resultant  statistical  distribution  of  any  nonlinear  function  of  track  variables  cannot  be 
determined  by  directly  transforming  the  track  mean  and  covariance  statistics,  even  when 
the  track  is  completely  characterised  by  its  mean  and  covariance.  To  illustrate  in  mathe¬ 
matical  terms,  if  x  is  a  random  variable  with  mean  /j  and  variance  cr2,  there  is,  in  general, 
no  closed-form  expression  for  the  mean  and  variance  of  a  nonlinear  function  h(x)  in  terms 
of  /i  and  cr2.  Note  that  h{fi)  is  not,  in  general,  the  mean  of  h{x). 

In  practice,  there  are  several  ways  of  estimating  the  statistics  of  nonlinearly  trans¬ 
formed  random  variables.  The  standard  method  is  to  approximate  the  transformation  by 
the  linear  function  obtained  by  truncating  its  Taylor  series  expanded  about  the  mean  of 
the  random  variables.  Another  approach,  which  has  been  applied  to  convert  measurements 
from  polar /spherical  to  Cartesian  coordinates  [2,  3,  4],  relies  on  trigonometric  identities.  A 
computationally-intensive  alternative  is  to  apply  the  Monte  Carlo  method  [5]  which  does 
not  require  tedious  closed-form  derivations.  In  this  report,  the  unscented  transformation 
[1]  is  applied.  Under  this  approach,  closed-form  derivations  are  unnecessary  and  computa¬ 
tional  requirements  are  at  least  an  order  of  magnitude  less  than  those  of  the  Monte  Carlo 
method.  Furthermore,  unlike  the  Monte  Carlo  method,  the  unscented  transformation  does 
not  require  a  pseudo-random  number  generator. 

In  the  next  section,  the  algorithm  based  on  the  unscented  transformation  is  described. 
In  Section  3,  example  results  are  provided  and  compared  with  Monte  Carlo  results.  The 
appendix  contains  Java  code  of  the  algorithm  for  generating  the  results.  The  conclusions 
are  given  in  Section  4. 


2  Algorithm 

The  basic  idea  of  the  unscented  transformation  is  to  choose  a  set  of  sigma  points  with 
assigned  weights  to  represent  a  multivariate  normal  distribution  that  preserves  the  mean 
and  covariance  [1] .  The  points  are  then  transformed,  and  the  transformed  points  are  used 
to  calculate  estimates  of  the  transformed  mean  and  covariance. 
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The  sigma  points,  x^°),  . . . ,  x-p),  and  their  weights,  W^°\  . . . ,  W&\  can  be  chosen  in  a 
number  of  ways  but  must  satisfy  [1]: 


Y  W{i)  =  1 

i=  0 

Y  fU(i)x(i)  =  x 
i= 0 

Y  w(<)(x(i)  -  x)(xW  -  x)T  =  P, 


4=0 


(1) 

(2) 

(3) 


where  x  and  Px  are  the  mean  and  covariance  of  the  target  track  state  x  respectively.  Under 
these  conditions,  the  unscented  transformation  approximates  the  mean  and  covariance  of 
h(x),  denoted  z,  by  [1] 

p 

z  =  Yw(i)z{i)  (4) 

4=0 

P 

PZ  =  Y  w(i)  (z  W  -  z)  (z(i)  -  z)T  (5) 

4=0 


where  z^  =  h(x^),  i  =  0, . . .  ,p. 

For  the  purpose  of  this  report,  the  following  symmetric  set  of  2 Nx  points  and  weights 


which  satisfy  conditions  (l)-(3)  are  chosen  [1]: 

XW  =  x  +  Cj  (6) 

W(i)  =  l/(2iVx)  (7) 

x(i+iVx)  =  x  -  a  (8) 

W(wx)  =  l/(2iVx)  (9) 


for  i  =  0, . . . ,  Nx  —  1,  where  iVx  is  the  number  of  dimensions  of  x  and  c,  is  the  (i  +  l)th 
column  of  the  Cholesky  decomposition  C  of  iVxPx,  i.e.,  A^XPX  =  CCT.  Note  that  Px 
must  be  a  symmetric  matrix  for  the  Cholesky  matrix  C  to  exist. 

The  set  of  2 Nx  points  can  be  extended  by  adding  another  point  equal  to  the  mean  x. 
This  extra  point  provides  a  parameter  for  controlling  higher  moments  of  the  distribution 
through  its  weighting.  More  details  are  given  by  Julier  and  Uhlmann  [1], 

The  entries  of  the  Cholesky  matrix  C  are  given  by  [6] : 


—  f  Pi, 3  -  ^2  Ci,kCj,k  )  ,  for  i  >  j 
™  \  k= l  J 


\ 


i— 1 


Pi,i  ~  ^2 


Ci,i  — 

v  i 

k= 1 

Ci,j  =  0,  for  i  <  j 


(10) 

(11) 

(12) 


2 


DSTO-TN-0745 


where  cty  is  the  zth  row,  jth  column  entry  of  C,  and  pt,j  is  the  ith  row,  jth  column  entry 
of  Px.  Note  that  C  is  a  lower  triangular  matrix. 

For  a  target  track  where  x  is  defined  as  (x,  y,  z,  x,  y,  z),  the  target  track  range  r,  bearing 
b,  elevation  e,  range  rate  r,  speed  s  and  course  c  are  given  by: 


■  =  \/  x2  +  y2  +  z2 

(13) 

b  =  arctan(x,  y) 

(14) 

e  =  arcsin(z/r) 

(15) 

=  (xx  +  yy  +  zz)/r 

(16) 

<M 

+ 

S- 

II 

CO 

(17) 

c  =  arctan(x,  y) 

(18) 

where  arctan(-,  •)  is  the  four  quadrant  arctangent  function.  Note  that,  in  this  report,  the 
arctangent  returns  bearing  and  course  relative  to  the  y-axis. 

The  equations  above  define  the  nonlinear  transformation  z  =  h(x)  =  (r,b,e,r,s,c). 
To  obtain  the  mean  and  covariance  of  z,  the  unscented  transformation  is  applied  with 
modifications  to  accommodate  the  directional  data,  i.e.,  bearing,  elevation  and  course.  If 
. . . ,  #09  represents  a  set  of  angles  falling  within  an  interval  of  length  27t,  then  the 
directional  mean  0  and  standard  deviation  ag  of  this  set  of  angles  are  given  by  [7]: 


9  =  arctan 


p 

i= 0 


C0 


i)  2 


i= 0 


(19) 

(20) 


where 


0(i) 


1 0W  -  6\  if  |0(i)  -  6\  <  7T, 

2tt  -  \e^  -  e\  if  \e^  -§\>ir. 


(21) 


Table  1  summarises  the  steps  to  obtain  range,  bearing,  elevation,  range  rate,  speed  and 
course  from  track  data  given  in  Cartesian  coordinates. 


3  Example 


The  algorithm  in  Table  1  was  implemented  in  Java  and  applied  to  the  following  track 
mean  and  covariance,  as  an  example. 


x  = 


100 

4 

5 

4 

3 

2 

1 

1 

P  = 

3 

4 

5 

4 

3 

2 

-10 

2 

3 

4 

5 

4 

3 

-5 

1 

2 

3 

4 

5 

4 

V  o  ) 

\o 

1 

2 

3 

4 

5/ 

2  1 


(31) 
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Table  1:  Steps  to  obtain  range,  bearing,  elevation,  range  rate,  speed  and  course  from  track 
data  given  in  Cartesian  coordinates 


Input:  Track  state  estimate  x  and  covariance  matrix  P  for  x  =  ( x ,  y,  z,  x,  y,  z) 
Output:  Range,  bearing,  elevation,  range  rate,  speed  and  course  estimates  and 
error  standard  deviations 

Step  1  IVX  =  6 

Step  2  Compute  Cholesky  matrix  entries  for  IVXPX: 

j- 1 


chj  ~ 


Ci,i  — 


,  ,  (  Pt.j  ^2  Ci,kcj,k 

jj  \  fe=i 


for  j  =  1,  •  •  • ,  i  -  1 


N 


i—  1 


Pi,i  -  ^2 c: 


i.k 


k= 1 


Ci,j  =  o,  for  j  =  i,...,Nx 
for  *  =  1, ... ,  IVX,  where  pij  is  the  (i,  j)th  entry  of  IVXP. 
Step  3  Create  sigma  points  x^1), . . . ,  x(2Wx): 


(22) 

(23) 

(24) 


XW  =  x  +  a 


X 


=  x  —  c, 


(25) 


for  i  =  1, ,  -/Vx,  where  Cj  is  the  ith  column  of  C. 

Step  4  Transform  each  sigma  point:  z^)  =  h(x^),  where  h(x)  is  given  by 

r  =  \J x2  +  y2  +  z2  b  =  arctan(x,  y)  e  =  arcsin  (z/r)  (26) 

r  =  (xx  +  yij  +  zz)/r  s  =  \J  x2  +  y2  c  =  arctan(x,  y)  (27) 

Step  5  Compute  mean  and  standard  deviation  for  range,  range  rate  and  speed 
using  the  formulae 


1 


x  = 


2 IV, 


2  Nx 

£ 

i=  1 


,(0 


(T  'Y'  — 


N 


2IV, 


1  2iVx 

—  5^(xW  -  x)2 


(28) 


2=1 


and  mean  and  standard  deviation  for  bearing,  elevation  and  course  using 
the  formulae 


/2JVX 

27VX  \ 

,  ^  COS  0^  J 

&0  —  \ 

V  2  =  1 

1=1  / 

\ 

2  Nx 


2Ny 


(29) 


i= 1 


where 


§{l)=(\e^-e\  ii\ew-e\<n, 

~\2ir-\0W-6\  if  |0W  _  >  7T. 


(30) 
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Table  2:  Results  from  Unscented  Transformation  (UT),  Monte  Carlo  simulations  (MC) 
and  the  track  mean  (TM) 


UT 

Error 

MC 

Error 

TM 

Range 

111.84 

2.88 

111.84 

2.90 

111.81 

Bearing 

0.46 

0.01 

0.46 

0.01 

0.46 

Elevation 

0.01 

0.02 

0.01 

0.02 

0.01 

Range  Rate 

-8.92 

2.88 

-8.95 

2.88 

-8.94 

Course 

-2.01 

0.12 

-2.01 

0.15 

-2.03 

Speed 

11.26 

2.85 

11.30 

2.82 

11.18 

The  code  is  given  in  the  appendix  of  this  report.  Table  2  compares  the  results  from 
the  algorithm  with  that  obtained  using  Monte  Carlo  simulations  (10,000  samples)  and 
with  estimates  based  on  the  track  mean.  From  the  results,  it  appears  that  the  unscented 
transformation  yields  results  that  are  in  close  agreement  with  those  from  Monte  Carlo 
simulations.  The  estimates  based  on  the  track  mean  are  also  in  close  agreement. 


4  Conclusions 

The  unscented  transformation  can  be  applied  to  tracks  given  in  Cartesian  coordinates, 
to  obtain  target  range,  bearing,  elevation,  range  rate,  speed  and  course  estimates,  and 
their  associated  errors.  This  approach  is  attractive  because  it  does  not  require  tedious 
closed-form  mathematical  derivations,  is  not  computationally  expensive,  and  does  not 
require  a  pseudo-random  number  generator.  Example  code  and  results  have  been  given  to 
demonstrate  the  applicability  of  the  approach. 
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Appendix  A  Example  Code 

The  following  example  code  is  written  in  Java.  Note  that  the  covariance  matrix  must 
be  symmetric  and  positive  definite  for  the  Cholesky  decomposition.  Divide  by  zero  can 
occur  if  the  covariance  matrix  is  degenerate. 

class  Example  { 

public  static  double  mean(double  []  x)  { 
double  m  =  0; 

for(int  i  =  0  ;  i  <  x. length  ;  i++) 
m  +=  x[i]  ; 

return  m  /  x. length; 

} 

public  static  double  stddev (double []  x,  double  m)  { 
double  ss  =  0; 
double  s; 

for(int  i  =  0  ;  i  <  x. length  ;  i++)  { 
s  =  x[i]  -  m; 
ss  +=  s  *  s; 

} 

return  Math.sqrt(ss  /  x. length); 

} 

public  static  double  dmean (double []  x)  { 

//  Mean  for  directional  data  given  in  radians 
double  s  =  0; 
double  c  =  0; 

for(int  i  =  0  ;  i  <  x. length  ;  i++)  { 
s  +=  Math.sin(x[i] ) ; 
c  +=  Math. cos (x  [i] ) ; 

} 

return  Math.atan2(s,  c) ; 

} 

public  static  double  dstddev (double []  x,  double  m)  { 

//  Standard  deviation  for  directional  data  given  in  radians 
//  Data  points  are  assumed  to  all  fall  in  the  same  2*pi  interval 
double  ss  =  0; 
double  s; 

double  TW0PI  =  2*Math.PI; 
for(int  i  =  0  ;  i  <  x. length  ;  i++)  { 
s  =  Math.abs(x[i]  -  m) ; 
if (s  >  Math. PI) 
s  =  TW0PI  -  s; 
ss  +=  s  *  s; 
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> 

return  Math.sqrt(ss  /  x. length); 

} 

public  static  void  compute_results (double  []  x,  double []  []  P)  { 

//  Number  of  dimensions 
int  n  =  x. length; 

//  Weight  covariance  matrix 

//  Note:  the  lower  triangular  part  of  the  covariance  matrix 
//is  over-written  in  the  process 
for(int  i  =  0  ;  i  <  n  ;  i++) 

for(int  j  =  0  ;  j  <=  i  ;  j++) 

P  [i]  [j]  =  n  *  P  [i]  [j]  ; 

//  Compute  lower-triangular  Cholesky  decomposition  using  the 
//  Cholesky-Banachiewicz  algorithm 
for(int  i  =  0  ;  i  <  n  ;  i++)  { 

for(int  j  =  0  ;  j  <  i  ;  j++)  { 

for (int  k  =  0  ;  k  <  j  ;  k++) 

P[i]  [j]  =  P[i]  [j]  -  P[i]  [k]  *  P [j]  [k]  ; 

P[i]  [j]  =  P[i]  [j]  /  P[j]  [j]; 

} 

for (int  k  =  0  ;  k  <  i  ;  k++) 

P[i]  [i]  =  P[i]  [i]  -  P[i]  [k]  *  P[i]  [k]  ; 

P[i][i]  =  Math . sqrt (P [i]  [i] ) ; 

} 

//  Create  sigma  points 

double  []  []  z  =  new  double  [n]  [2*n]  ; 

for(int  i  =  0  ;  i  <  n  ;  i++)  { 

for(int  j  =  0  ;  j  <=  i  ;  j++)  { 

z[i]  [j]  =  x[i]  +  P[i]  [j]; 
z[i]  [n+j]  =  x[i]  -  P[i]  [j]; 

} 

for(int  j=i+l;j<n;  j++)  { 

//  Note  that  for  Cholesky  decomposition,  entries  above 
//  diagonal  are  zero 
z[i]  [j]  =  x[i]  ; 
z  [i]  [n+j]  =  x  [i]  ; 

} 

> 

//  Transform  sigma  points 

//  Sigma  points  are  assumed  to  be  3D  Cartesian  position  and  velocity 
doublet]  r  =  new  double  [2*n] ;  //  Slant  range 
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doublet]  b  =  new  double [2*n] ;  //  Bearing  relative  to  y-axis  (radians) 

doublet]  e  =  new  double [2*n] ;  //  Elevation  (radians) 

double  t]  rr  =  new  double  t2*n] ;  //  Slant  range  rate 

doublet]  c  =  new  double  [2*n] ;  //  Course  relative  to  y-axis  (radians) 

doublet]  s  =  new  double  [2*n] ;  //  Speed  in  x-y  plane 

for(int  i  =  0  ;  i  <  2*n  ;  i++)  { 

rti]  =  Math,  sqrt  (z  [0]  [i]  *z  [0]  [i]  +  z  tl]  ti]  *z  tl]  ti]  +  z  [2]  [i]  *z  [2]  [i] ) ; 
bti]  =  Math.  atan2 (z  t0]  ti]  ,  z[l]fi]); 
e  ti]  =  Math.  asin(z  t2]  ti]  /  rti]); 

rr  ti]  =  (ztO]  [i]*z[3]  ti]  +  z  [1]  [i]  *z  [4]  [i]  +  z  t2]  [i]  *z  t5]  [i] )  /  rti]; 
c  ti]  =  Math.  atan2(z  [3]  ti]  >  zt4]ti]); 
s  ti]  =  Math,  sqrt  (z  [3]  [i]  *z  [3]  [i]  +  z  [4]  [i]  *z  [4]  [i] ) ; 

> 

//  Determine  slant  range  and  error  (standard  deviation) 

double  range  =  mean(r) ; 

double  rangeError  =  stddev(r,  range); 

//  Determine  bearing  and  error  (standard  deviation) 

double  bearing  =  dmean(b) ; 

double  bearingError  =  dstddev(b,  bearing); 

//  Determine  elevation  and  error  (standard  deviation) 
double  elevation  =  dmean(e) ; 

double  elevationError  =  dstddev(e,  elevation); 

//  Determine  slant  range  rate  and  error  (standard  deviation) 
double  rangeRate  =  mean(rr) ; 

double  rangeRateError  =  stddev(rr,  rangeRate) ; 

//  Determine  course  and  error  (standard  deviation) 

double  course  =  dmean(c) ; 

double  courseError  =  dstddev(c,  course); 

//  Determine  speed  and  error  (standard  deviation) 

double  speed  =  mean(s) ; 

double  speedError  =  stddev(s,  speed); 

System. out . format ("Range  =  %7.2f,  ",  range); 

System,  out .  format  ("Error  =  °/07 . 2f  °/0n" ,  rangeError); 

System. out. format ("Bearing  =  %7.2f,  ",  bearing); 

System,  out.  format  ("Error  =  °/07 . 2f  °/0n" ,  bearingError); 

System. out. format ("Elevation  =  %7.2f,  ",  elevation); 

System,  out.  format  ("Err  or  =  °/07 . 2f  °/0n" ,  elevationError); 

System. out. format ("Range  Rate  =  %7.2f,  ",  rangeRate); 

System,  out.  format  ("Error  =  °/07 . 2f  °/0n" ,  rangeRateError); 

System. out. format ("Course  =  %7.2f,  ",  course); 
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System,  out.  format  ("Error  =  °/07 . 2f  °/0n" ,  courseError) ; 
System. out. format ("Speed  =  °/07.2f,  ",  speed); 

System,  out.  format  ("Error  =  °/07 . 2f  °/0n" ,  speedError) ; 
System,  out  .printlnO  ; 

System. out .println("Cholesky  Decomposition") ; 
for(int  i  =  0  ;  i  <  n  ;  i++)  { 

for(int  j  =  0  ;  j  <=  i  ;  j++) 

System,  out.  format  ("°/07.2f  ",  P  [i]  [  j  ] ) ; 
System,  out  .printlnO  ; 

> 

System,  out  .printlnO  ; 

System. out .println("Sigma  Points") ; 
for(int  i  =  0  ;  i  <  n  ;  i++)  { 

for(int  j  =  0  ;  j  <  2*n  ;  j++) 

System,  out.  format  ("°/07.2f  ",  z  [i]  [  j  ] ) ; 
System,  out  .printlnO  ; 

} 


public  static  void  main(String []  args)  { 

//  Example  mean 

double  []  x  =  {50,  100,  1,  -10,  -5,  0}; 

//  Example  covariance  matrix 
//  This  must  be  a  symmetric  matrix 
doublet]  []  P  =  {  {5,  4,  3,  2,  1,  0>, 
{4,  5,  4,  3,  2,  1>, 
{3,  4,  5,  4,  3,  2>, 
{2,  3,  4,  5,  4,  3>, 
{1,  2,  3,  4,  5,  4>, 
{0,  1,  2,  3,  4,  5}  }; 

compute_results(x,  P) ; 

} 

> 
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